#include "PID.h"
#include "PID_Control.h"
#include "DevicePropeller.h"

// 主线程退出处理函数
void cleanup_and_exit(int signum) {
    DevicePropeller devicePropeller("can3","can");
    Device<Propeller> &device_propeller = devicePropeller;

    printf("主线程收到终止信号 %d，准备退出...\n", signum);
    
    // fl = 1;

    sleep(3);
    propeller.angle = 0;

    propeller.mode = 1;
    device_propeller.func(propeller);

    propeller.mode = 2;
    device_propeller.func(propeller);

    propeller.mode = 3;
    device_propeller.func(propeller);

    printf("完成\n");
    
    
    exit(EXIT_SUCCESS);
}

int ToPoint(float end_x, float end_y, Device<Propeller> &device_propeller){
    // 注册信号处理函数
    signal(SIGINT, cleanup_and_exit);

    // 使用PID进行控制
    
    // 初始化两个PID控制器 
    // 控制器参数赋值：比例/积分/微分增益、微分时间常数、输出限幅、积分限幅、采样时间
    // 用于偏航角控制
    PIDController pid = { 
        PID_heading_KP, PID_heading_KI, PID_heading_KD,
        PID_heading_TAU,
        PID_heading_LIM_MIN, PID_heading_LIM_MAX,
		PID_heading_LIM_MIN_INT, PID_heading_LIM_MAX_INT,
        SAMPLE_TIME_S,0.0f,0.0f,0.0f,0.0f,0.0f };
    PIDController_Init(&pid);  // 控制器初始化

    // 用于速度控制
    PIDController pid2 = { 
        PID_velocity_KP, PID_velocity_KI, PID_velocity_KD,
        PID_velocity_TAU,
        PID_velocity_LIM_MIN, PID_velocity_LIM_MAX,
		PID_velocity_LIM_MIN_INT, PID_velocity_LIM_MAX_INT,
        SAMPLE_TIME_S ,0.0f,0.0f,0.0f,0.0f,0.0f};
    PIDController_Init(&pid2);  // 控制器初始化
    
    // 打印表头：时间、系统输出、控制器输出
    // printf("Time(s)\tSystem Output\tControllerOutput\r\n");
    
    // 循环执行：从0秒开始，每次递增采样时间，直到达到最小误差深度
    for (float t = 0.0f; ; t += SAMPLE_TIME_S) {
        // 获取当前坐标
        float start_x = aipov_data.longitude;  // x
        float start_y = aipov_data.latitude; // y
        // printf("start_x:%d, start_y:%d\n",start_x,start_y);

        // 获取当前偏航角
        float measurement = aipov_data.heading;

        // 计算角度（弧度）,并转换为角度
        double angle_deg = calculate_angle(start_x, start_y, end_x, end_y)* (180.0 / PI);

        // 计算控制信号 输入当前偏航角 输出尾推角度
        PIDController_Update(&pid, angle_deg, measurement);  // 调用PID更新函数，根据目标值和测量值计算新的控制量

        // 将pid控制的输出值作为尾推角度传递给尾推 
        // 尾推的实际旋转角度×10 = 指令角度
        // 二次保护，尾推的旋转角度不能大于180
        int out = (int) (pid.out * -10);
        propeller.angle = Limit_Size(out,Propeller_LR_Max,Propeller_LR_Min);

        propeller.mode = 2;
        device_propeller.func(propeller);
        // // 打印当前时间、系统输出、控制器输出（制表符分隔，便于表格化显示）
        // printf("%.2f\t%f\t%f\r\n", t, measurement, pid1.out);

        double distance = calculate_distance(start_x, start_y, end_x, end_y);
        // 计算控制信号 输入与目标点的距离 输出尾推螺旋桨转速
        PIDController_Update(&pid2, 0, distance);  // 调用PID更新函数，根据目标值和测量值计算新的控制量
        out = (int) pid2.out;

        propeller.angle = Limit_Size(out,Propeller_Go_Max,Propeller_Go_Min);

        propeller.mode = 1;
        device_propeller.func(propeller);
        
        // 当距离足够接近时，停止输出
        if(distance < 0.1){
            break;
        }

        usleep(50000);
    }

    return 0;
}

int ToDepth(float setpoint, Device<Propeller> &device_propeller){
    signal(SIGINT, cleanup_and_exit);

    // 使用双环PID进行控制，外环为深度控制（控制器1），内环为俯仰角控制（控制器2）
    
    // 初始化两个PID控制器 
    // 控制器参数赋值：比例/积分/微分增益、微分时间常数、输出限幅、积分限幅、采样时间
    PIDController pid1 = { 
        PID1_KP, PID1_KI, PID1_KD,
        PID1_TAU,
        PID1_LIM_MIN, PID1_LIM_MAX,
		PID1_LIM_MIN_INT, PID1_LIM_MAX_INT,
        SAMPLE_TIME_S,0.0f,0.0f,0.0f,0.0f,0.0f };

    PIDController_Init(&pid1);  // 控制器1初始化

    PIDController pid2 = { 
        PID2_KP, PID2_KI, PID2_KD,
        PID2_TAU,
        PID2_LIM_MIN, PID2_LIM_MAX,
		PID2_LIM_MIN_INT, PID2_LIM_MAX_INT,
        SAMPLE_TIME_S,0.0f,0.0f,0.0f,0.0f,0.0f };

    PIDController_Init(&pid2);  // 控制器2初始化

    // 螺旋桨控制
    PIDController pid3 = { 
        PID_velocity_KP, PID_velocity_KI, PID_velocity_KD,
        PID_velocity_TAU,
        PID_velocity_LIM_MIN, PID_velocity_LIM_MAX,
		PID_velocity_LIM_MIN_INT, PID_velocity_LIM_MAX_INT,
        SAMPLE_TIME_S ,0.0f,0.0f,0.0f,0.0f,0.0f};
    PIDController_Init(&pid3);  // 控制器初始化

    // 打印表头：时间、系统输出、控制器输出
    // printf("Time(s)\tSystem Output\tControllerOutput\r\n");
    
    // 循环执行：从0秒开始，每次递增采样时间，直到达到最小误差深度
    printf("measurement\tsetpoint\terror\tout\n");
    for (float t = 0.0f; ; t += SAMPLE_TIME_S) {
        float measurement1 = depth_data.depth;  // 获取深度
        float measurement2 = aipov_data.pitch; // 获取俯仰角

        // 计算外环的控制信号 输入当前深度 输出俯仰角目标值
        PIDController_Update(&pid1, setpoint, measurement1);  // 调用PID更新函数，根据目标值和测量值计算新的控制量

        // 计算内环的控制信号 输入当前俯仰角 输出尾推的俯仰角
        PIDController_Update(&pid2, pid1.out, measurement2);

        // 计算内环的控制信号 输入当前深度 输出尾推的螺旋桨转速
        PIDController_Update(&pid3, setpoint, measurement1);  // 调用PID更新函数，根据目标值和测量值计算新的控制量


        // 将pid控制的输出值作为尾推角度传递给尾推 
        // 尾推的实际旋转角度×10 = 指令角度
        // 二次保护，尾推的旋转角度不能大于180
        int out = (int) (pid2.out * 10);
        propeller.angle = Limit_Size(out,Propeller_UD_Max,Propeller_UD_Min);

        propeller.mode = 1;
        device_propeller.func(propeller);

        out = (int) pid3.out;
        propeller.angle = Limit_Size(out,Propeller_Go_Max,Propeller_Go_Min);

        propeller.mode = 3;
        device_propeller.func(propeller);
        // 打印当前时间、系统输出、控制器输出（制表符分隔，便于表格化显示）
        printf("%f\t%f\t%f\t%d\n",measurement1,setpoint,measurement1-setpoint,out);

        // if((setpoint - measurement1) < 0.01 && (setpoint - measurement1) > -0.01){
        //     break;
        // }
    }
    
    return 0;
}

int ToHeading(float setpoint, Device<Propeller>& device_propeller){
    signal(SIGINT, cleanup_and_exit);

    // 使用PID进行控制
    
    // 初始化PID控制器 
    // 控制器参数赋值：比例/积分/微分增益、微分时间常数、输出限幅、积分限幅、采样时间
    // 用于偏航角控制
    PIDController pid = { 
        PID_heading_KP, PID_heading_KI, PID_heading_KD,
        PID_heading_TAU,
        PID_heading_LIM_MIN, PID_heading_LIM_MAX,
		PID_heading_LIM_MIN_INT, PID_heading_LIM_MAX_INT,
        SAMPLE_TIME_S ,0.0f,0.0f,0.0f,0.0f,0.0f};
    PIDController_Init(&pid);  // 控制器初始化
    
    // 螺旋桨控制
    PIDController pid1 = { 
        PID_velocity_KP, PID_velocity_KI, PID_velocity_KD,
        PID_velocity_TAU,
        PID_velocity_LIM_MIN, PID_velocity_LIM_MAX,
		PID_velocity_LIM_MIN_INT, PID_velocity_LIM_MAX_INT,
        SAMPLE_TIME_S ,0.0f,0.0f,0.0f,0.0f,0.0f};
    PIDController_Init(&pid1);  // 控制器初始化

    // 打印表头：时间、系统输出、控制器输出
    printf("measurement\tsetpoint\terror\tout\n");
    
    // 循环执行：从0秒开始，每次递增采样时间，直到达到最小误差深度
    for (float t = 0.0f; ; t += SAMPLE_TIME_S) {
        // 获取当前偏航角
        float measurement = aipov_data.heading;

        // 计算控制信号 输入当前偏航角 输出尾推角度
        PIDController_Update(&pid, setpoint, measurement);  // 调用PID更新函数，根据目标值和测量值计算新的控制量
        PIDController_Update(&pid1, setpoint, measurement);  // 调用PID更新函数，根据目标值和测量值计算新的控制量

        int out = (int) (pid.out * -10);
        
        // 将pid控制的输出值作为尾推角度传递给尾推 
        // 尾推的实际旋转角度×10 = 指令角度
        // 二次保护，尾推的旋转角度不能大于180
        propeller.angle = Limit_Size(out,Propeller_LR_Max,Propeller_LR_Min);
        
        propeller.mode = 2;
        device_propeller.func(propeller);

        out = (int)pid1.out;
        propeller.angle = Limit_Size(out,Propeller_Go_Max,Propeller_Go_Min);
        
        propeller.mode = 3;
        device_propeller.func(propeller);

        printf("%f\t%f\t%f\t%d\n",measurement,setpoint,measurement-setpoint,out);
        usleep(50000);
    }

    return 0;
}

int ToVelocity(float setpoint, Device<Propeller>& device_propeller){
    signal(SIGINT, cleanup_and_exit);

    // 使用PID进行控制
    
    // 初始化PID控制器 
    // 控制器参数赋值：比例/积分/微分增益、微分时间常数、输出限幅、积分限幅、采样时间
    // 用于速度控制
    PIDController pid1 = { 
        PID_velocity_KP, PID_velocity_KI, PID_velocity_KD,
        PID_velocity_TAU,
        PID_velocity_LIM_MIN, PID_velocity_LIM_MAX,
		PID_velocity_LIM_MIN_INT, PID_velocity_LIM_MAX_INT,
        SAMPLE_TIME_S ,0.0f,0.0f,0.0f,0.0f,0.0f};
    PIDController_Init(&pid1);  // 控制器初始化
    
    // 打印表头：时间、系统输出、控制器输出
    // printf("Time(s)\tSystem Output\tControllerOutput\r\n");
    
    propeller.mode = 3;

    cout << "measurement\tsetpoint\terror\tout" << endl;
    // 循环执行：从0秒开始，每次递增采样时间，直到达到最小误差深度
    for (float t = 0.0f; ; t += SAMPLE_TIME_S) {
        // 获取当前速度
        float measurement = aipov_data.velocity;

        // 计算控制信号 输入当前速度 输出尾推螺旋桨角度
        PIDController_Update(&pid1, setpoint, measurement);  // 调用PID更新函数，根据目标值和测量值计算新的控制量

        // 将pid控制的输出值作为尾推角度传递给尾推 
        // 二次保护，尾推的旋转角度不能大于180
        int out = (int) (pid1.out);
        propeller.angle = Limit_Size(out,Propeller_Go_Max,Propeller_Go_Min);

        device_propeller.func(propeller);
        printf("%f\t%f\t%f\t%d\n",measurement,setpoint,measurement-setpoint,out);

        usleep(50000);
    }

    return 0;
}
